#include "agx_arm_task/rm_task.hpp"

arm_task_node::arm_task_node()
{
  //init ros
  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");

  //service
  set_mode = private_nh.advertiseService("set_mode_srv",
                               &arm_task_node::set_mode_callback,this);
  velo_move_arm_service = private_nh.advertiseService("velo_move_arm_srv",
                               &arm_task_node::velo_move_arm_callback,this);
  move_arm_service = private_nh.advertiseService("move_arm_srv",
                               &arm_task_node::move_arm_callback,this);
  clear_error_service = private_nh.advertiseService("clear_error_srv",
                               &arm_task_node::clear_error_callback,this);
  move_tool_motor_service = private_nh.advertiseService("move_tool_motor",
                               &arm_task_node::move_tool_motor_callback,this);
  //init state flags
  _robot_state_t._robot_enabled = false;
  
}

void arm_task_node::subcallback::robot_state_callback(const sensor_msgs::JointState& msg)
{
  std::string name_space = _id;
  //update the arm state struct
//   _ptr->_robot_state_t._robot_mode = msg.mode;
//   _ptr->_robot_state_t._robot_state = msg.state;
//   _ptr->_robot_state_t.error_code = msg.err;
//   _ptr->_robot_state_t.warn_code = msg.warn;
  
  //transform msg and pub it
  bt_task_msgs::arm_state state_msg;
  state_msg.id = name_space;
  state_msg.header = msg.header;
  state_msg.arm_name = "agx_realman_63";
//   state_msg.mode = msg.mode;
//   state_msg.state = msg.state;
//   state_msg.error_code = msg.err;
for(int i=0;i<6;i++)
{
  state_msg.joint_angle.emplace_back(msg.position[i]);
}
//   state_msg.tcp_position = msg.pose;

  //transform radius to angle
  float radius_factor = (180/3.14159);
  for (size_t i = 0; i < 6; i++)
  {
    float& angle = state_msg.joint_angle.at(i);
    //radius to angle
    angle *= radius_factor;
    //保留一位小数
    angle = (int)(10.0 * angle + 0.5) / 10.0;
  }
    // std::cout<<state_msg.joint_angle[0]<<std::endl;


  auto pub_iter = _ptr->xarm_state_pub_map.find(name_space);
  if(pub_iter == _ptr->xarm_state_pub_map.end())
  {
    ROS_ERROR("state callback: invalid id called : %s",name_space.c_str());
  }
  else
  {
    if(_last_state.id == state_msg.id &&
        _last_state.arm_name == state_msg.arm_name &&
        _last_state.mode == state_msg.mode &&
        _last_state.state == state_msg.state &&
        _last_state.error_code == state_msg.error_code &&
        _last_state.joint_angle == state_msg.joint_angle &&
        _last_state.tcp_position == state_msg.tcp_position)
    {
      ROS_DEBUG("we got the same msg");
      return;
    }
    else
    {
      ROS_DEBUG("we got the diffirent msg");
      pub_iter->second.publish(state_msg);
      _last_state = state_msg;

      if_get_joint_state = true;
    }
  }
}

bool arm_task_node::set_mode_callback(bt_task_msgs::set_arm::Request& request,
                                  bt_task_msgs::set_arm::Response& response)
{
  ROS_INFO("\033[32mSERVICE CALL: realman don't have set_arm\033[0m");
  // init arm
  std::string name_space = request.id;
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    init_rm(name_space);
    ROS_INFO("init arm with id: %s", name_space.c_str());
    xarm_already_init_list.emplace(name_space);
    // response.success = true;
    // return true;
  }

  response.success = true;
  response.message = "success";
  response.code = 12006;
  return true;
}

bool arm_task_node::velo_move_arm_callback(bt_task_msgs::velo_move_arm::Request& request,
                      bt_task_msgs::velo_move_arm::Response& response)
{
  ROS_INFO("\033[32mSERVICE CALL: velo_move_arm\033[0m");
  std::string name_space = request.id;

  //if id exist
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    ROS_ERROR("SERVICE velo_move_arm: cant find arm id: %s",name_space.c_str());
    response.success = false;
    response.message = "SERVICE velo_move_arm: cant find arm id";
    response.code = 12001;
    return true;
  }

  float joint_speed = request.joint_speed;
  if(joint_speed == 0)
  {
    response.success = true;
    response.message = "success";
    response.code = 12006;
    return true;
  }
  //transform radius to angle
  float radius_factor = (3.14159/180);
    //radius to angle
    joint_speed *= radius_factor;

  std::vector<float> joint_speed_vec;
  for (size_t i = 0; i < 6; i++)
  {
    joint_speed_vec.emplace_back(0);
  }

  joint_speed_vec[joint_name_map[request.joint_name]] = joint_speed;

  //api
  // if we get joint state, we can move joint.if not we abord
  if(sub_callback_map[name_space]->if_get_joint_state == true)
  {
    std::vector<float> current_angle;
    current_angle = sub_callback_map[name_space]->_last_state.joint_angle;

    //transform angle to radius
    float radius_factor = (3.14159/180);
    for (size_t i = 0; i < 6; i++)
    {
        float& angle = current_angle.at(i);
        //radius to angle
        angle *= radius_factor;
    }
    _xarm_client_ptr_map[name_space]->veloMoveJoint(joint_speed_vec,current_angle);
  }
  else
  {
    ROS_WARN("SERVICE velo_move_arm: if_get_joint_state is false");
  }
  //return
  response.success = true;
  response.message = "success";
  response.code = 12006;
  return true;
}

bool arm_task_node::move_arm_callback(bt_task_msgs::move_arm::Request& request,
                      bt_task_msgs::move_arm::Response& response)
{
  ROS_INFO("\033[32mSERVICE CALL: move_arm\033[0m");
  // init arm
  std::string name_space = request.id;
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    init_rm(name_space);
    ROS_INFO("init arm with id: %s", name_space.c_str());
    xarm_already_init_list.emplace(name_space);
    //waiting for init
    ros::Duration(0.5).sleep();
    // response.success = true;
    // return true;
  }

  //if id exist
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    ROS_ERROR("SERVICE move_arm: cant find arm id: %s",name_space.c_str());
    response.success = false;
    response.message = "SERVICE move_arm: cant find arm id";
    response.code = 12002;

    return true;
  }
  
  //storage target joint angle
  std::vector<float> target_joint = request.joint_angle;

  //transform angle to radius
  float radius_factor = (3.14159/180);
  for (size_t i = 0; i < 6; i++)
  {
    float& angle = request.joint_angle.at(i);
    //radius to angle
    angle *= radius_factor;
  }

  std::vector<float> joint_angle_set;
  joint_angle_set.emplace_back(request.joint_angle[0]);
  joint_angle_set.emplace_back(request.joint_angle[1]);
  joint_angle_set.emplace_back(request.joint_angle[2]);
  joint_angle_set.emplace_back(request.joint_angle[3]);
  joint_angle_set.emplace_back(request.joint_angle[4]);
  joint_angle_set.emplace_back(request.joint_angle[5]);

  //api
  _xarm_client_ptr_map[name_space]->moveJoint(joint_angle_set,0.1);
  
  //judge if arm is arrived
  int8_t timeout = 0;
  std::vector<float>& current_joint =  sub_callback_map[name_space]->_last_state.joint_angle;
  while(abs(current_joint[0] - target_joint[0]) >= 0.2 ||
        abs(current_joint[1] - target_joint[1]) >= 0.2 ||
        abs(current_joint[2] - target_joint[2]) >= 0.2 ||
        abs(current_joint[3] - target_joint[3]) >= 0.2 ||
        abs(current_joint[4] - target_joint[4]) >= 0.2 ||
        abs(current_joint[5] - target_joint[5]) >= 0.2)
  {
    ros::Duration(1).sleep();
    timeout++;
    //timeout,return false
    if(timeout>10)
    {
      response.success = false;
      response.message = "SERVICE move_arm: timeout waiting for arrival";
      response.code = 12004;
      return true;
    }
  }
  //return
  response.success = true;
  response.message = "success";
  response.code = 12006;

  return true;
}

bool arm_task_node::clear_error_callback(bt_task_msgs::clear_error::Request& request,
                      bt_task_msgs::clear_error::Response& response)
{
  ROS_INFO("\033[32mSERVICE CALL: clear error\033[0m");
  std::string name_space = request.id;
  //if id exist
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    ROS_ERROR("SERVICE clear_error: cant find arm id: %s",name_space.c_str());
    response.success = false;
    response.message = "SERVICE clear_error: cant find arm id";
    response.code = 12003;
    return true;
  }

  _xarm_client_ptr_map[name_space]->clearErr();
  ROS_INFO("ARM_TASK: clear error");

  //return
  response.success = true;
  response.message = "success";
  response.code = 12006;
  return true;
}

bool arm_task_node::move_tool_motor_callback(bt_task_msgs::move_tool_motor::Request& request,
                      bt_task_msgs::move_tool_motor::Response& response)
{
  ROS_INFO("\033[32mSERVICE CALL: move motor\033[0m");
  std::string name_space = request.id;
  //if id exist
  if(xarm_already_init_list.find(name_space) == xarm_already_init_list.end())
  {
    ROS_ERROR("SERVICE move motor: cant find arm id: %s",name_space.c_str());
    response.success = false;
    response.message = "SERVICE move motor: cant find arm id";
    response.code = 12003;
    return true;
  }

  if(request.run == false)
  {
    _xarm_client_ptr_map[name_space]->run_tcp_motor(false);
  }
  else
  {
    _xarm_client_ptr_map[name_space]->start_run_motor(request.clockwise,request.speed);
  }
  
  //return
  response.success = true;
  response.message = "success";
  response.code = 12006;
  return true;
}
void arm_task_node::init_rm(const std::string id)
{
  //init ros handler
  std::string name_space = id;
  ros::NodeHandle arm_nh(name_space);
  ros::NodeHandle pub_nh("/agx_arm_task_node");

  ros::Time begin=ros::Time::now(); 
  //init xarm client ptr
  _xarm_client_ptr_map[id] = std::make_shared<RealmanRobotClient>();
  auto iter = _xarm_client_ptr_map.find(id);
  if(iter != _xarm_client_ptr_map.end())
  {
    auto client_ptr = iter->second;
    client_ptr->init(arm_nh);
  }

  //init publisher and subscriber
  xarm_state_pub_map[id] = pub_nh.advertise<bt_task_msgs::arm_state>("arm_state", 10, true);

  sub_callback_map[id] = std::make_shared<subcallback>(arm_nh,shared_from_this(),id);

  robot_state_sub_map[id] = arm_nh.subscribe("joint_states", 1,
      &subcallback::robot_state_callback, sub_callback_map[id].get());

  ros::Duration running_tim = ros::Time::now() - begin;
  ROS_INFO("\033[32mSERVICE CALL: init xarm,use time(ms): %f\033[0m",(float)(running_tim.nsec)/1000000.0);
}